// 2011-03-27 dgl created
#include "coreintegr.h"
#include <math.h>

#ifdef OMP_ON
  #include <omp.h>
#endif
#include <stdio.h>



int RadauIIA(TOdeSys* system, TMethodData* data){ 
  const double s6 = sqrt(6.0);
  const double c1=(4.0-s6)/10.0, c2=(4.0+s6)/10.0, c3=1.0;
	const double a11=(88.0-7.0*s6)/360.0, a12=(296.0-169.0*s6)/1800.0, a13=(-2.0+3.0*s6)/225.0;
  const double a21=(296.0+169.0*s6)/1800.0, a22=(88.0+7.0*s6)/360.0, a23=(-2.0-3.0*s6)/225.0;
  const double a31=(16.0-s6)/36.0, a32=(16.0+s6)/36.0, a33 = 1.0/9.0;
   

  int i,j,k;
  //compute r1
	system->func(system->time, system->state, system->params, data->stor[0],0,system->dim-1, system->dim);
  //compute r2  
  
  for (j=1;j<3; j++)
    for(i=0;i<system->dim; i++)
      data->stor[j][i] = data->stor[0][i];

  for (k=1;k<=4;k++){
    for(i=0;i<system->dim; i++){
		  data->stor[3][i] = system->state[i] + data->step*(a11*data->stor[0][i]+a12*data->stor[1][i]+a13*data->stor[2][i]);
      data->stor[4][i] = system->state[i] + data->step*(a21*data->stor[0][i]+a22*data->stor[1][i]+a23*data->stor[2][i]);
      data->stor[5][i] = system->state[i] + data->step*(a31*data->stor[0][i]+a32*data->stor[1][i]+a33*data->stor[2][i]);
    }          
    system->func(system->time+data->step*c1, data->stor[3], system->params, data->stor[0],0,system->dim-1, system->dim); 
    system->func(system->time+data->step*c2, data->stor[4], system->params, data->stor[1],0,system->dim-1, system->dim); 
    system->func(system->time+data->step*c3, data->stor[5], system->params, data->stor[2],0,system->dim-1, system->dim); 
  }

  //increase time
  system->time = system->time + data->step;
  //x_{n+1}=x_n+h*(r1/6+r2/3+r3/3+r4/6)
  for(i=0;i<system->dim; i++)
	  system->state[i] += data->step*(a31*data->stor[0][i]+ a32*data->stor[1][i] + a33*data->stor[2][i]);
	data->accepted++;
//debug
/*	for(i=0;i<system->dim; i++)
	      system->state[i] = data->stor[0][i];*/
//end debug 
	return C_SUCCESS;
}


//====----Dormand-Prince 5(4)----====\\

static double min_d (double a, double b)
{
  return (a < b)?a:b;

} /* min_d */


static double max_d (double a, double b)
{
  return (a > b)?a:b;

} /* max_d */
int DoPri54_iter(TOdeSys* system, TMethodData* data)
{ 
	const double c2=0.2, c3=0.3, c4=0.8, c5=8.0/9.0;
	const double a21=0.2, a31=3.0/40.0, a32=9.0/40.0;
    const double a41=44.0/45.0, a42=-56.0/15.0, a43=32.0/9.0;
    const double a51=19372.0/6561.0, a52=-25360.0/2187.0;
    const double a53=64448.0/6561.0, a54=-212.0/729.0;
    const double a61=9017.0/3168.0, a62=-355.0/33.0, a63=46732.0/5247.0;
    const double a64=49.0/176.0, a65=-5103.0/18656.0;
    const double a71=35.0/384.0, a73=500.0/1113.0, a74=125.0/192.0;
    const double a75=-2187.0/6784.0, a76=11.0/84.0;

    const double e1=71.0/57600.0, e3=-71.0/16695.0, e4=71.0/1920.0;
    const double e5=-17253.0/339200.0, e6=22.0/525.0, e7=-1.0/40.0;
#ifdef __DGL_DEBUG
	const double b1=5179.0/57600.0, b3=7571.0/16695.0, b4=393.0/640.0;
    const double b5=-92097.0/339200.0, b6=187.0/2100.0, b7=1.0/40.0;
#endif
	//const double d1=-12715105075.0/11282082432.0, d3=87487479700.0/32700410799.0;
    //const double d4=-10690763975.0/1880347072.0, d5=701980252875.0/199316789632.0;
    //const double d6=-1453857185.0/822651844.0, d7=69997945.0/29380423.0;
	const double facmin=0.5, facmax = 2, fac = 0.9;
	int i, accepted=0;
	double err, erri, *tmp;
	double step; 

	while (!accepted)
	{
	step = data->step;
	if (step<10e-9)
		return C_STEPTOOSMALL;
	if (data->accepted > 2*10e6)
		return C_TOOMANYSTEPS;
    //compute r1
	system->func(system->time, system->state, system->params, data->stor[1],0,system->dim-1, system->dim);
    //compute r2  
    for(i=0;i<system->dim; i++)
		data->stor[0][i] = system->state[i] + step*a21*data->stor[1][i];
    system->func(system->time+c2*data->step, data->stor[0], system->params, data->stor[2],0,system->dim-1, system->dim); 
    //compute r3
    for(i=0;i<system->dim; i++)
		data->stor[0][i] = system->state[i] + step*(a31*data->stor[1][i] + a32*data->stor[2][i]);
    system->func(system->time+c3*data->step, data->stor[0], system->params, data->stor[3],0,system->dim-1, system->dim);     
	//compute r4
    for(i=0;i<system->dim; i++)
		data->stor[0][i] = system->state[i] + step*(a41*data->stor[1][i] + a42*data->stor[2][i] + a43*data->stor[3][i]);
    system->func(system->time+c4*data->step, data->stor[0], system->params, data->stor[4],0,system->dim-1, system->dim);         
	//compute r5
    for(i=0;i<system->dim; i++)
		data->stor[0][i] = system->state[i] + step*(a51*data->stor[1][i] + a52*data->stor[2][i] +
			a53*data->stor[3][i] + a54*data->stor[4][i]);
    system->func(system->time+c5*data->step, data->stor[0], system->params, data->stor[5],0,system->dim-1, system->dim);         
	//compute r6
    for(i=0;i<system->dim; i++)
		data->stor[0][i] = system->state[i] + step*(a61*data->stor[1][i] + a62*data->stor[2][i] +
			a63*data->stor[3][i] + a64*data->stor[4][i] + a65*data->stor[5][i]);
    system->func(system->time+data->step, data->stor[0], system->params, data->stor[6],0,system->dim-1, system->dim);         
	//compute r7 (and in data->stor[0] we get the possible y_{n+1})
    for(i=0;i<system->dim; i++)
		data->stor[0][i] = system->state[i] + step*(a71*data->stor[1][i] + a73*data->stor[3][i] +
			a74*data->stor[4][i] + a75*data->stor[5][i] + a76*data->stor[6][i]);
    system->func(system->time+data->step, data->stor[0], system->params, data->stor[7],0,system->dim-1, system->dim);         
	//compute error
	err = 0;
	for (i=0; i<system->dim; i++)
	{
		erri = step*(e1*data->stor[1][i] + e3*data->stor[3][i] + e4*data->stor[4][i] + 
							e5*data->stor[5][i] + e6*data->stor[6][i]+e7*data->stor[7][i]) 
							/(data->atol+data->rtol*max_d(data->stor[0][i], system->state[i]));
		//(data->atol+data->rtol*max_d(fabs(data->stor[0][i]), fabs(system->state[i])));
/*#ifdef __DGL_DEBUG
		fprintf(data->log,"diff%d is %6.12f\n", i, e1*data->stor[1][i] + e3*data->stor[3][i] + e4*data->stor[4][i] + 
							e5*data->stor[5][i] + e6*data->stor[6][i]+e7*data->stor[7][i]);
#endif*/
		//printf("sc%d is %6.12f\n", i, (data->atol+data->rtol*max_d(data->stor[0][i], system->state[i])));
		err+=erri*erri;
	}
  //  printf("data-stor1[0] =%6.12f\n", data->stor[1][0]);
	//fprintf(data->log,"step = %f\n",step);
	//for (i=1;i<=7;i++)
	//	fprintf(data->log,"K%d = (%.20f, %.20f, %.20f)\n",i,data->stor[i][0],data->stor[i][1],data->stor[i][2]);
/*#ifdef __DGL_DEBUG
	fprintf(data->log,"y_n+1 = (%.20f, %.20f, %.20f)\n",data->stor[0][0],data->stor[0][1],data->stor[0][2]);
	for (i=0; i<system->dim; i++)
	{data->stor[0][i] = system->state[i] + step*(b1*data->stor[1][i] + b3*data->stor[3][i] +
			b4*data->stor[4][i] + b5*data->stor[5][i] + b6*data->stor[7][i]+b7*data->stor[7][i]);
	}
	fprintf(data->log,"y_n+1 = (%.20f, %.20f, %.20f)\n",data->stor[0][0],data->stor[0][1],data->stor[0][2]);
#endif*/
	err = sqrt(err/system->dim);
	//now compute the next step size
	//fprintf(data->log,"err = %1.20f\n 1/err = %f\n (1/err)^(1/8) = %f\n",err, 1.0/err,pow(1.0/err,1.0/8.0));
    //printf("factor =  %f\n",min_d(facmax,max_d(facmin,fac*pow(1/err,1.0/8.0))));
    
	data->step = step * min_d(facmax,max_d(facmin,fac*pow(1/err,1.0/5.0)));

	if (err<1)
	{
		//we already have the result in data->stor[0]
		system->time = system->time + step;
		tmp = system->state;
		system->state = data->stor[0];
		data->stor[0] = tmp;
		accepted = 1;
		data->accepted++;
	}
	else
	{
		data->rejected++;
	}
	}//end while      
                
//debug
/*	for(i=0;i<system->dim; i++)
	      system->state[i] = data->stor[0][i];*/
//end debug 
	return C_SUCCESS;
}
